
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_radio.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <notify/notify.h>
#include <common/time/gp_time.h>
#include <srv_channel/srv_channel.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  check if throttle value is within allowed range
 */
bool fms_rc_throttle_value_ok(void)
{
    if ((enum ThrFailsafe)(fms.g.throttle_fs_enabled) == TFS_Disabled) {
        return true;
    }
    if (rc_get_reverse(fms.channel_throttle )) {
        return rc_get_radio_in(fms.channel_throttle) < fms.g.throttle_fs_value;
    }
    return rc_get_radio_in(fms.channel_throttle) > fms.g.throttle_fs_value;
}

/*
  return true if throttle level is below throttle failsafe threshold
  or RC input is invalid
 */
bool fms_rc_failsafe_active(void)
{
    if (!fms_rc_throttle_value_ok()) {
        return true;
    }
    if (time_millis() - fms.failsafe.last_valid_rc_ms > 1000) {
        // we haven't had a valid RC frame for 1 seconds
        return true;
    }
    return false;
}

/*
  expo handling for MANUAL, ACRO and TRAINING modes
 */
static float channel_expo(rc_channel_t chan, int8_t expo, bool use_dz)
{
    if (chan == NULL) {
        return 0;
    }
    float rin = use_dz? rc_get_control_in(chan) : rc_get_control_in_zero_dz(chan);
    return SERVO_MAX * math_expo_curve(math_constrain_float(expo*0.01f, 0, 1), rin/SERVO_MAX);
}

float fms_roll_in_expo(bool use_dz)
{
    return channel_expo(fms.channel_roll, fms.g.man_expo_roll, use_dz);
}

float fms_pitch_in_expo(bool use_dz)
{
    return channel_expo(fms.channel_pitch, fms.g.man_expo_pitch, use_dz);
}

float fms_rudder_in_expo(bool use_dz)
{
    return channel_expo(fms.channel_rudder, fms.g.man_expo_rudder, use_dz);
}

/*
  allow for runtime change of control channel ordering
 */
void fms_set_control_channels(void)
{
    rc_mapper_t rcmapper = rc_mapper_get_singleton();

    if (fms.g.rudder_only) {
        // in rudder only mode the roll and rudder channels are the
        // same.
        fms.channel_roll = rcs_chan(rcmapper->ch_yaw-1);
    } else {
        fms.channel_roll = rcs_chan(rcmapper->ch_roll-1);
    }
    fms.channel_pitch    = rcs_chan(rcmapper->ch_pitch-1);
    fms.channel_throttle = rcs_chan(rcmapper->ch_throttle-1);
    fms.channel_rudder   = rcs_chan(rcmapper->ch_yaw-1);

    // set rc channel ranges
    rc_set_angle(fms.channel_roll, SERVO_MAX);
    rc_set_angle(fms.channel_pitch, SERVO_MAX);
    rc_set_angle(fms.channel_rudder, SERVO_MAX);
    if (!fms_have_reverse_thrust()) {
        // normal operation
        rc_set_angle(fms.channel_throttle, 100);
    } else {
        // reverse thrust
        if (fms.have_reverse_throttle_rc_option) {
            // when we have a reverse throttle RC option setup we use throttle
            // as a range, and rely on the RC switch to get reverse thrust
            rc_set_angle(fms.channel_throttle, 100);
        } else {
            rc_set_angle(fms.channel_throttle, 100);
        }
        srv_channels_set_angle(k_throttle, 100);
        srv_channels_set_angle(k_throttleLeft, 100);
        srv_channels_set_angle(k_throttleRight, 100);
    }

    // update flap and airbrake channel assignment
    fms.channel_flap     = rcs_find_channel_for_option(RC_AUX_FLAP);
    fms.channel_airbrake = rcs_find_channel_for_option(RC_AUX_AIRBRAKE);

#if HAL_QUADPLANE_ENABLED
    // update manual forward throttle channel assignment
    quadplane.rc_fwd_thr_ch = rcs_find_channel_for_option(RC_AUX_FWD_THR);
#endif

    bool set_throttle_esc_scaling = true;
#if HAL_QUADPLANE_ENABLED
    set_throttle_esc_scaling = !quadplane.enable;
#endif
    if (set_throttle_esc_scaling) {
        // setup correct scaling for ESCs like the UAVCAN ESCs which
        // take a proportion of speed. For quadplanes we use AP_Motors
        // scaling
        srv_channels_set_esc_scaling_for(k_throttle);
    }
}

/*
  initialise RC input channels
 */
void fms_init_rc_in()
{
    rc_mapper_init();

    rcs_init();
    fms_set_control_channels();

    // set rc dead zones
    rc_set_default_dead_zone(fms.channel_roll, 30);
    rc_set_default_dead_zone(fms.channel_pitch, 30);
    rc_set_default_dead_zone(fms.channel_rudder, 30);
    rc_set_default_dead_zone(fms.channel_throttle, 30);
}

/*
  initialise RC output for main channels. This is done early to allow
  for BRD_SAFETY_DEFLT=0 and early servo control
 */
void fms_init_rc_out_main()
{
    /*
      change throttle trim to minimum throttle. This prevents a
      configuration error where the user sets CH3_TRIM incorrectly and
      the motor may start on power up
     */
    if (!fms_have_reverse_thrust()) {
        srv_channels_set_trim_to_min_for(k_throttle, false);
        srv_channels_set_trim_to_min_for(k_throttleLeft, false);
        srv_channels_set_trim_to_min_for(k_throttleRight, false);
    }

    srv_channels_set_failsafe_limit(k_aileron, SRV_TRIM);
    srv_channels_set_failsafe_limit(k_elevator, SRV_TRIM);
    srv_channels_set_failsafe_limit(k_throttle, SRV_TRIM);
    srv_channels_set_failsafe_limit(k_throttleLeft, SRV_TRIM);
    srv_channels_set_failsafe_limit(k_throttleRight, SRV_TRIM);
    srv_channels_set_failsafe_limit(k_rudder, SRV_TRIM);

}

/*
  initialise RC output channels for aux channels
 */
void fms_init_rc_out_aux()
{
    srv_channels_enable_aux_servos();

    fms_servos_output();

    // setup PWM values to send if the FMU firmware dies
    // allows any VTOL motors to shut off
    srv_channels_setup_failsafe_trim_all_non_motors();
}

/*
  check for pilot input on rudder stick for arming/disarming
*/
void fms_rudder_arm_disarm_check()
{
    const int16_t rudder_in = rc_get_control_in(fms.channel_rudder);
    if (rudder_in == 0) {
        // remember if we've seen neutral rudder, used for VTOL auto-takeoff
        fms.seen_neutral_rudder = true;
    }
    if (!mb_arming_is_armed()) {
        // when not armed, full right rudder starts arming counter
        if (rudder_in > 4000) {
            uint32_t now = time_millis();

            if (fms.rudder_arm_timer == 0 ||
                now - fms.rudder_arm_timer < 3000) {

                if (fms.rudder_arm_timer == 0) {
                    fms.rudder_arm_timer = now;
                }
            } else {
                //time to arm!
                mb_arming_arm(ARMING_METHOD_RUDDER, true);
                fms.rudder_arm_timer = 0;
                fms.seen_neutral_rudder = false;
                fms.takeoff_state.rudder_takeoff_warn_ms = now;
            }
        } else {
            // not at full right rudder
            fms.rudder_arm_timer = 0;
        }
    } else {
        // full left rudder starts disarming counter
        if (rudder_in < -4000) {
            uint32_t now = time_millis();

            if (fms.rudder_arm_timer == 0 ||
                now - fms.rudder_arm_timer < 3000) {
                if (fms.rudder_arm_timer == 0) {
                    fms.rudder_arm_timer = now;
                }
            } else {
                //time to disarm!
                mb_arming_disarm(ARMING_METHOD_RUDDER, true);
                fms.rudder_arm_timer = 0;
            }
        } else {
            // not at full left rudder
            fms.rudder_arm_timer = 0;
        }
    }
}

void fms_read_radio()
{
    if (!rcs_read_input()) {
        fms_control_failsafe();
        return;
    }

    if (!fms.failsafe.rc_failsafe)
    {
        fms.failsafe.AFS_last_valid_rc_ms = time_millis();
    }

    if (fms_rc_throttle_value_ok()) {
        fms.failsafe.last_valid_rc_ms = time_millis();
    }

    fms_control_failsafe();

#if AP_FENCE_ENABLED
    const bool stickmixing = fence_stickmixing();
#else
    const bool stickmixing = true;
#endif
    fms.airspeed_nudge_cm = 0;
    fms.throttle_nudge = 0;
    if (fms.g.throttle_nudge
        && rc_get_control_in(fms.channel_throttle) > 50
        && stickmixing) {
        float nudge = (rc_get_control_in(fms.channel_throttle) - 50) * 0.02f;
        if (ahrs_airspeed_sensor_enabled(get_ahrs_view())) {
            fms.airspeed_nudge_cm = (fms.aparm.airspeed_max * 100 - fms.aparm.airspeed_cruise_cm) * nudge;
        } else {
            fms.throttle_nudge = (fms.aparm.throttle_max - fms.aparm.throttle_cruise) * nudge;
        }
    }

    fms_rudder_arm_disarm_check();

#if HAL_QUADPLANE_ENABLED
    // potentially swap inputs for tailsitters
    quadplane.tailsitter.check_input();
#endif

    // check for transmitter tuning changes
#if 0 //TODO: ?
    tuning.check_input(control_mode->mode_number());
#endif
}

void fms_control_failsafe()
{
    if (fms_rc_failsafe_active()) {
        // we do not have valid RC input. Set all primary channel
        // control inputs to the trim value and throttle to min
        rc_set_radio_in(fms.channel_roll, rc_get_radio_trim(fms.channel_roll));
        rc_set_radio_in(fms.channel_pitch, rc_get_radio_trim(fms.channel_pitch));
        rc_set_radio_in(fms.channel_rudder, rc_get_radio_trim(fms.channel_rudder));

        // note that we don't set channel_throttle->radio_in to radio_trim,
        // as that would cause throttle failsafe to not activate
        rc_set_control_in(fms.channel_roll, 0);
        rc_set_control_in(fms.channel_pitch, 0);
        rc_set_control_in(fms.channel_rudder, 0);

        fms.airspeed_nudge_cm = 0;
        fms.throttle_nudge = 0;

        switch (mode_number(fms.control_mode)) {
#if HAL_QUADPLANE_ENABLED
            case MN_QSTABILIZE:
            case MN_QHOVER:
            case MN_QLOITER:
            case MN_QLAND: // throttle is ignored, but reset anyways
            case MN_QRTL:  // throttle is ignored, but reset anyways
            case MN_QACRO:
#if QAUTOTUNE_ENABLED
            case MN_QAUTOTUNE:
#endif
                if (quadplane.available() && quadplane.motors->get_desired_spool_state() > AP_Motors::DesiredSpoolState::GROUND_IDLE) {
                    // set half throttle to avoid descending at maximum rate, still has a slight descent due to throttle deadzone
                    channel_throttle->set_control_in(channel_throttle->get_range() / 2);
                    break;
                }
                FALLTHROUGH;
#endif
            default:
                rc_set_control_in(fms.channel_throttle, 0);
                break;
        }
    }

    const bool allow_failsafe_bypass = !mb_arming_is_armed() && !fms_is_flying() && (rcs_enabled_protocols() != 0);
    const bool has_had_input = rcs_has_had_rc_receiver() || rcs_has_had_rc_override();
    if (((enum ThrFailsafe)(fms.g.throttle_fs_enabled) != TFS_Enabled) || (allow_failsafe_bypass && !has_had_input)) {
        // If not flying and disarmed don't trigger failsafe until RC has been received for the fist time
        return;
    }

    if (fms_rc_failsafe_active()) {
        // we detect a failsafe from radio
        // throttle has dropped below the mark
        fms.failsafe.throttle_counter++;
        if (fms.failsafe.throttle_counter == 10) {
            gcs_send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "on");
            fms.failsafe.rc_failsafe = true;
            notify_flags.failsafe_radio = true;
        }
        if (fms.failsafe.throttle_counter > 10) {
            fms.failsafe.throttle_counter = 10;
        }
    } else if(fms.failsafe.throttle_counter > 0) {
        // we are no longer in failsafe condition
        // but we need to recover quickly
        fms.failsafe.throttle_counter--;
        if (fms.failsafe.throttle_counter > 3) {
            fms.failsafe.throttle_counter = 3;
        }
        if (fms.failsafe.throttle_counter == 1) {
            gcs_send_text(MAV_SEVERITY_WARNING, "Throttle failsafe %s", "off");
        } else if(fms.failsafe.throttle_counter == 0) {
            fms.failsafe.rc_failsafe = false;
            notify_flags.failsafe_radio = false;
        }
    }
}

bool fms_throttle_at_zero(void)
{
/* true if throttle stick is at idle position...if throttle trim has been moved
   to center stick area in conjunction with sprung throttle, cannot use in_trim, must use rc_min
*/
    if (((!(fms_flight_option_enabled(FO_CENTER_THROTTLE_TRIM) && rc_in_trim_dz(fms.channel_throttle))) ||
        (fms_flight_option_enabled(FO_CENTER_THROTTLE_TRIM)&& rc_in_min_dz(fms.channel_throttle)))) {
        return true;
    }
    return false;
}

int16_t fms_rudder_input(void)
{
    if (fms.g.rudder_only != 0) {
        // in rudder only mode we discard rudder input and get target
        // attitude from the roll channel.
        return 0;
    }

    if ((fms_flight_option_enabled(FO_DIRECT_RUDDER_ONLY)) &&
        !(fms.control_mode == (mode_base_t)&fms.mode_manual || fms.control_mode == (mode_base_t)&fms.mode_stabilize || fms.control_mode == (mode_base_t)&fms.mode_acro)) {
        // the user does not want any input except in these modes
        return 0;
    }

    if (fms_stick_mixing_enabled()) {
        return rc_get_control_in(fms.channel_rudder);
    }

    return 0;
}
/*------------------------------------test------------------------------------*/


